#include "mainwindow.h"

void TemDataThread::run()
{
    sensor_data_node_cpp = rclcpp::Node::make_shared("sensor_data_node");
    // 订阅
    tem_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "tem_talker",10,std::bind(&TemDataThread::tem_data_cb, this, std::placeholders::_1));
    thickness_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "thickness_talker",10,std::bind(&TemDataThread::thickness_data_cb,this,std::placeholders::_1));
    roughness_lis =sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "roughness_talker",10,std::bind(&TemDataThread::roughness_data_cb,this,std::placeholders::_1));
    voltage_array_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Float64MultiArray>(
        "voltage_array_2",10,std::bind(&TemDataThread::voltage_array_data_cb,this,std::placeholders::_1));
    wheel_status_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "wheel_status",10,std::bind(&TemDataThread::wheel_status_cd,this,std::placeholders::_1));
    arm_status_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "arm_status",10,std::bind(&TemDataThread::arm_status_cd,this,std::placeholders::_1));
    joystick_or_cruising_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "joystick_or_cruising",10,std::bind(&TemDataThread::joystick_or_cruising_cb,this,std::placeholders::_1));
    cruising_speed_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "cruising_speed",10,std::bind(&TemDataThread::cruising_speed_cd,this,std::placeholders::_1));
    distance_lis = sensor_data_node_cpp->create_subscription<sensor_msgs::msg::Range>(
        "/laser", 10, std::bind(&TemDataThread::range_callback, this, std::placeholders::_1));
    armpos_lis = sensor_data_node_cpp->create_subscription<std_msgs::msg::Float32>(
        "/degree", 10, std::bind(&TemDataThread::degree_callback, this, std::placeholders::_1));
    armpos_lis_2 = sensor_data_node_cpp->create_subscription<std_msgs::msg::Float32>(
        "/rt_position", 10, std::bind(&TemDataThread::degree_callback_2, this, std::placeholders::_1));
    // spin阻塞
    rclcpp::spin(sensor_data_node_cpp);
}
// armpos
void TemDataThread::degree_callback(const std_msgs::msg::Float32 degree_msg)
{
    armpos_emit(degree_msg.data);
}
void TemDataThread::degree_callback_2(const std_msgs::msg::Float32 degree_msg)
{
    armpos_emit_2(degree_msg.data);
}
// distance
void TemDataThread::range_callback(const sensor_msgs::msg::Range::SharedPtr msg)
{
    // laser距离值，单位：m
    float distance = msg->range;
    if (distance > 65)
    {
        distance = 0.000;
    }
    distance = distance * 100;
    distance_emit(distance);
}

// 温度传感器
void TemDataThread::tem_data_cb(const std_msgs::msg::Int32 tem_msg)
{
    int data = tem_msg.data;
    tem_emit(data);
}
// 漆膜仪
void TemDataThread::thickness_data_cb(const std_msgs::msg::Int32 thickness_msg)
{
    int data = thickness_msg.data;
    thickness_emit(data);
}
// 粗糙度
void TemDataThread::roughness_data_cb(const std_msgs::msg::Int32 roughness_msg)
{
    int data = roughness_msg.data;
    roughness_emit(data);
}
// 电压数组
void TemDataThread::voltage_array_data_cb(const std_msgs::msg::Float64MultiArray voltage_array_msg)
{
    float voltage_1 = voltage_array_msg.data[0];
    float voltage_2 = voltage_array_msg.data[1];
    wheel_voltage_emit(voltage_1);
    arm_voltage_emit(voltage_2);
}
// wheel推杆状态
void TemDataThread::wheel_status_cd(const std_msgs::msg::Int32 msg)
{
    wheel_status_emit(msg.data);
}
void TemDataThread::arm_status_cd(const std_msgs::msg::Int32 msg)
{
    arm_status_emit(msg.data);
}
// 巡航或遥杆
void TemDataThread::joystick_or_cruising_cb(const std_msgs::msg::Int32 msg)
{
    joystick_or_cruising_emit(msg.data);
}
// 巡航速度
void TemDataThread::cruising_speed_cd(const std_msgs::msg::Int32 msg)
{
    float temp = (msg.data * 360.0 * 3.14*0.125)/(100 * 110 * 3);
    cruising_speed_emit(temp);
}
